#ifndef QFE_H
#define QFE_H

#include <stdint.h>

#include <hexagon_types.h>
#include <hexagon_protos.h>
#include <hvx_hexagon_protos.h>

typedef int16_t hfq15_t; // [-0.5, 0.5)
#define hfq15_from_float(x)    ((hfq15_t)Q6_R_sath_R(Q6_R_convert_sf2w_R((x) * 32768.0f)))
// #define hfq15_to_float(x)      ((float)((x) / 32768.0f))
#define hfq15_to_float(x)      ((float)(qhmath_div_f(x, 32768.0f)))
#define hfq15_const_half()       hfq15_from_float(0.5f)
#define hfq15_const_e()          ((hfq15_t)(1))
#define hfq15_const_1_m_e()      (hfq15_const_1 - fq15_const_e)
// #define hfq15_mul(x, y)        ((hfq15_t)((((int32_t)(x)) * ((int32_t)(y))) >> 15))
#define hfq15_mul(x, y)        ((hfq15_t)(Q6_R_mpy_RlRl_sat((x), (y))>>15))
// #define hfq15_div(x, y)        ((hfq15_t)((((int32_t)(x)) << 15) / (y)))
#define hfq15_div(x, y)        ((hfq15_t)((((int32_t)(x)) << 15) / (y)))
#define hfq15_const_max()        ((int32_t)0x7fffffff)
#define hfq15_const_min()        ((int32_t)0x80000000)

typedef int32_t wfq11_t;
#define wfq11_from_int(i)      (((wfq11_t)(i)) << 11)
#define wfq11_from_float(x)    ((wfq11_t)Q6_R_convert_sf2w_R((x) * (1<<11)))
#define wfq11_to_float(x)      ((float)(qhmath_div_f(x, (1<<11))))
#define wfq11_const_1()        (wfq11_from_int(1))
#define wfq11_mul(x, y)        ((wfq11_t)(Q6_P_mpy_RR((x), (y))>>11))
#define wfq11_div(x, y)        ((wfq11_t)((((int64_t)(x))<<11)/(y)))


typedef int32_t wfq15_t;
#define wfq15_from_int(i)      (((wfq15_t)(i)) << 15)
#define wfq15_from_float(x)    ((wfq15_t)Q6_R_convert_sf2w_R((x) * 32768.0f))
#define wfq15_to_float(x)      ((float)(qhmath_div_f(x, 32768.0f)))
#define wfq15_const_1()        (wfq15_from_int(1))
#define wfq15_mul(x, y)        ((wfq15_t)(Q6_P_mpy_RR((x), (y))>>15))
#define wfq15_div(x, y)        ((wfq15_t)((((int64_t)(x))<<15)/(y)))


typedef int32_t wfq16_t;
#define wfq16_from_int(i)      (((wfq16_t)(i)) << 16)
#define wfq16_from_float(x)    ((wfq16_t)Q6_R_convert_sf2w_R((x) * 65536.0f))
#define wfq16_to_float(x)      ((float)(qhmath_div_f(x, 65536.0f)))
#define wfq16_const_1()        (wfq16_from_int(1))
#define wfq16_mul(x, y)        ((wfq16_t)(Q6_P_mpy_RR((x), (y))>>16))
#define wfq16_div(x, y)        ((wfq16_t)((((int64_t)(x))<<16)/(y)))


typedef int32_t wfq18_t;
#define wfq18_from_int(i)      (((wfq18_t)(i)) << 18)
#define wfq18_from_float(x)    ((wfq18_t)Q6_R_convert_sf2w_R((x) * (1<<18)))
#define wfq18_to_float(x)      ((float)(qhmath_div_f(x, (1<<18))))
#define wfq18_const_1()        (wfq18_from_int(1))
#define wfq18_mul(x, y)        ((wfq18_t)(Q6_P_mpy_RR((x), (y))>>18))
#define wfq18_div(x, y)        ((wfq18_t)((((int64_t)(x))<<18)/(y)))


typedef int32_t wfq8_t;
#define wfq8_from_int(i)      (((wfq8_t)(i)) << 8)
#define wfq8_from_float(x)    ((wfq8_t)Q6_R_convert_sf2w_R((x) * 256.0f))
#define wfq8_to_float(x)      ((float)(qhmath_div_f(x, 256.0f)))
#define wfq8_const_1()        (wfq8_from_int(1))
#define wfq8_mul(x, y)        ((wfq8_t)(Q6_P_mpy_RR((x), (y))>>8))
#define wfq8_div(x, y)        ((wfq8_t)((((int64_t)(x))<<8)/(y)))

typedef int32_t wfq7_t;
#define wfq7_from_int(i)      (((wfq7_t)(i)) << 7)
#define wfq7_from_float(x)    ((wfq7_t)Q6_R_convert_sf2w_R((x) * 128.0f))
#define wfq7_to_float(x)      ((float)(qhmath_div_f(x, 128.0f)))
#define wfq7_const_1()        (wfq7_from_int(1))
#define wfq7_mul(x, y)        ((wfq7_t)(Q6_P_mpy_RR((x), (y))>>7))
#define wfq7_div(x, y)        ((wfq7_t)((((int64_t)(x))<<7)/(y)))



typedef int32_t chfq15_t;
#define chfq15_re(v) (Q6_R_sxth_R(v))
#define chfq15_im(v) (Q6_R_asrh_R(v))


typedef int16_t hfq8_t;
#define hfq8_from_int(i)      (((hfq8_t)(i)) << 8)
#define hfq8_from_float(x)    ((hfq8_t)((x) * 256.0f))
#define hfq8_to_float(x)      ((float)(qhmath_div_f(x, 256.0f)))
#define hfq8_mul(x, y)        ((hfq8_t)(Q6_R_mpy_RlRl((x), (y))>>8))
#define hfq8_div(x, y)        ((hfq8_t)((((int32_t)(x))<<8)/(y)))


#define hfq7_to_float(x)      ((float)(qhmath_div_f(x, 128.0f)))

typedef int16_t chfq8_t;
#define chfq8_re(v) (Q6_R_sxtb_R(v))
#define chfq8_im(v) (v>>8)

typedef int32_t chfq_t;

inline void chfq15_to_float(chfq15_t q, float &re, float &im)
{
    hfq15_t v1 = chfq15_re(q);
    hfq15_t v2 = chfq15_im(q);
    re = hfq15_to_float(v1);
    im = hfq15_to_float(v2);
}
inline void chfq8_to_float(chfq8_t q, float &re, float &im)
{
    hfq8_t v1 = chfq8_re(q);
    hfq8_t v2 = chfq8_im(q);
    re = hfq8_to_float(v1);
    im = hfq8_to_float(v2);
}


#if 0
typedef int32_t Q8Complex;
typedef int64_t Q15Complex;

//v2 -- rQ8
inline Q8Complex complex_Q8_add_cQ8_rQ8(Q8Complex v1, int16_t v2)
{
    int32_t v = Q6_R_sxth_R(v1);
    v = Q6_R_add_RR_sat(v, v2);
    return Q6_R_combine_RhRl(v1, v);
}

//v2 -- Q7.8
inline Q8Complex complex_Q8_mul_cQ8_rQ8(Q8Complex v1, int16_t v2)
{
    int32_t v = Q6_R_sxth_R(v1);
    v = Q6_R_mpy_RlRl_sat(v, v2)>>8;
    return Q6_R_combine_RhRl(v1, v);
}

//(a+bi)*(c+di) = (a*c-b*d)+(b*c+a*d)i
inline Q8Complex complex_cQ8_mul_cQ8_cQ8(Q8Complex v1, Q8Complex v2)
{
    int64_t v = Q6_R_cmpy_RR_rnd_sat(v1, v2);
    v1 = static_cast<int32_t>(v&0x0ffffffff) >> 8;
    v2 = static_cast<int32_t>(v >> 32) >> 8;
    return Q6_R_combine_RlRl(v2, v1);
}

inline Q8Complex complex_cQ8_mul_conj_cQ8_cQ8(Q8Complex v1, Q8Complex v2)
{
    int64_t v = Q6_R_cmpy_RR_conj_rnd_sat(v1, v2);
    v1 = static_cast<int32_t>(v&0x0ffffffff) >> 8;
    v2 = static_cast<int32_t>(v >> 32) >> 8;
    return Q6_R_combine_RlRl(v2, v1);
}

//(a+bi)/(c+di) = (a*c+b*d)/(c*c+d*d) + (b*c-a*d)/(c*c+d*d)i
inline Q8Complex complex_cQ8_div_cQ8_cQ8(Q8Complex v1, Q8Complex v2)
{
    int32_t c2 = Q6_R_mpy_RlRl_sat(v2, v2) >> 8;
    int32_t d2 = Q6_R_mpy_RhRh_sat(v2, v2) >> 8;
    int32_t ac = Q6_R_mpy_RlRl_sat(v1, v2) >> 8;
    int32_t bd = Q6_R_mpy_RhRh_sat(v1, v2) >> 8;
    int32_t bc = Q6_R_mpy_RhRl_sat(v1, v2) >> 8;
    int32_t ad = Q6_R_mpy_RlRh_sat(v1, v2) >> 8;
    int32_t v = Q6_R_add_RR_sat(c2, d2);
    int32_t v_1 = Q6_R_add_RR_sat(ac, bd);
    int32_t v_2 = Q6_R_sub_RR_sat(bc, ad);
    v1 = (v_1 << 8)/v; // real
    v2 = (v_2 << 8)/v; // imag

    return Q6_R_combine_RlRl(v2, v1);
}


//v2 -- rQ15
inline Q15Complex complex_cQ15_add_cQ8_rQ15(Q8Complex v1, int16_t v2)
{
    int32_t real = Q6_R_sxth_R(v1) << 7;
    int32_t imag = Q6_R_sxth_R((v1>>16)) << 7; 
    real = Q6_R_add_RR_sat(real, v2);
    return  Q6_P_combine_RR(imag, real);
}


//(a+bi)*(c+di) = (a*c-b*d)+(b*c+a*d)i
inline Q15Complex complex_cQ15_mul_cQ15_cQ15(Q15Complex v1, Q15Complex v2)
{
    int64_t v = Q6_P_cmpy_RR_sat(v1, v2);
    int32_t v_1 = static_cast<int32_t>(v & 0x0ffffffff); // real
    int32_t v_2 = static_cast<int32_t>(v >> 32); // imag
    return ((v_1&0x0ffff) | ((v_2<<16)&0x0ffff0000));
}

//(a+bi)/(c+di) = (a*c+b*d)/(c*c+d*d) + (b*c-a*d)/(c*c+d*d)i
inline Q15Complex complex_cQ15_div_cQ8_cQ15(Q8Complex v1, Q15Complex v2)
{
    int32_t v1_re = Q6_R_sxth_R(v1&0x0ffff) << 7; // Q8-->Q15
    int32_t v1_im = (v1>>16) << 7;  // Q8-->Q15
    int32_t v2_re = static_cast<int32_t>(v2&0x0ffffffff);
    int32_t v2_im = static_cast<int32_t>(v2>>32);
    int32_t c2 = fq15_mul(v2_re, v2_re);
    int32_t d2 = fq15_mul(v2_im, v2_im);
    int32_t ac = fq15_mul(v1_re, v2_re);
    int32_t bd = fq15_mul(v1_im, v2_im);
    int32_t bc = fq15_mul(v1_im, v2_re);
    int32_t ad = fq15_mul(v1_re, v2_im);
    int32_t v =  Q6_R_add_RR_sat(c2, d2);
    int32_t v_1 = fq15_div(Q6_R_add_RR_sat(ac, bd), v);
    int32_t v_2 = fq15_div(Q6_R_sub_RR_sat(bc, ad), v);

    return Q6_P_combine_RR(v_2, v_1);
}
#endif
// typedef union{
//     int64_t vv;
//     struct {
//         wfq15_t re;
//         wfq15_t im;
//     } v;
// } cwfq15_t;
// #define cwfq15_re(v)     (v.v.re)
// #define cwfq15_im(v)     (v.v.im)

typedef int64_t cwfq11_t;
#define cwfq11_re(v)        ((wfq11_t)((v)))
#define cwfq11_im(v)        ((wfq11_t)((v)>>31))


typedef int64_t cwfq17_t;
#define cwfq17_re(v)        ((wfq16_t)((v)))
#define cwfq17_im(v)        ((wfq16_t)((v)>>31))

typedef int64_t cwfq16_t;
#define cwfq16_re(v)        ((wfq16_t)((v)))
#define cwfq16_im(v)        ((wfq16_t)((v)>>32))

typedef int64_t cwfq18_t;
#define cwfq18_re(v)        ((wfq18_t)((v)))
#define cwfq18_im(v)        ((wfq18_t)((v)>>32))

typedef int64_t cwfq8_t;
#define cwfq8_re(v)        ((wfq8_t)((v)))
#define cwfq8_im(v)        ((wfq8_t)((v)>>32))

typedef int64_t cwfq7_t;
#define cwfq7_re(v)        ((wfq7_t)((v)))
#define cwfq7_im(v)        ((wfq7_t)((v)>>32))

inline void cwfq7_to_float(cwfq7_t q, float &re, float &im)
{
    wfq8_t v1 = cwfq7_re(q);
    wfq8_t v2 = cwfq7_im(q);
    re = wfq7_to_float(v1);
    im = wfq7_to_float(v2);
}


inline void cwfq8_to_float(cwfq8_t q, float &re, float &im)
{
    wfq8_t v1 = cwfq8_re(q);
    wfq8_t v2 = cwfq8_im(q);
    re = wfq8_to_float(v1);
    im = wfq8_to_float(v2);
}
inline void cwfq11_to_float(cwfq11_t q, float &re, float &im)
{
    wfq11_t v1 = cwfq11_re(q);
    wfq11_t v2 = cwfq11_im(q);
    re = wfq11_to_float(v1);
    im = wfq11_to_float(v2);
}
inline void cwfq18_to_float(cwfq18_t q, float &re, float &im)
{
    wfq18_t v1 = cwfq18_re(q);
    wfq18_t v2 = cwfq18_im(q);
    re = wfq18_to_float(v1);
    im = wfq18_to_float(v2);
}

inline cwfq16_t cwfq16_from_float(float re, float im)
{
    wfq16_t v1 = wfq16_from_float(re);
    wfq16_t v2 = wfq16_from_float(im);
    return Q6_P_combine_RR(v2, v1);
}

inline cwfq16_t cwfq16_from_int(int re, int im)
{
    wfq16_t v1 = wfq16_from_int(re);
    wfq16_t v2 = wfq16_from_int(im);
    return Q6_P_combine_RR(v2, v1);
    // return v1 + (((int64_t)v2)<<32);
}

inline void cwfq16_to_float(cwfq16_t q, float &re, float &im)
{
    wfq16_t v1 = cwfq16_re(q);
    wfq16_t v2 = cwfq16_im(q);
    re = wfq16_to_float(v1);
    im = wfq16_to_float(v2);
}

inline cwfq16_t cwfq16_add_re(cwfq16_t v1, wfq16_t v2)
{
    wfq16_t v1_re = cwfq16_re(v1)+v2;
    return (v1&~0x0ffffffff) | v1_re;
}

inline cwfq16_t cwfq16_add_im(cwfq16_t v1, wfq16_t v2)
{
    wfq16_t v1_im = cwfq16_im(v1)+v2;
    return (v1&0x0ffffffff) | (((int64_t)(v1_im))<<32);
}
//(a+bi)*(c+di)=(ac-bd)+(ad+bc)i
inline cwfq16_t cwfq16_mul(cwfq16_t v1, cwfq16_t v2)
{
    wfq16_t v1_re = cwfq16_re(v1);
    wfq16_t v1_im = cwfq16_im(v1);
    wfq16_t v2_re = cwfq16_re(v2);
    wfq16_t v2_im = cwfq16_im(v2);
    int64_t ac = Q6_P_mpy_RR(v1_re, v2_re);
    int64_t bd = Q6_P_mpy_RR(v1_im, v2_im);
    int64_t bc = Q6_P_mpy_RR(v1_im, v2_re);
    int64_t ad = Q6_P_mpy_RR(v1_re, v2_im);
    int64_t re = (ac-bd) >> 16;
    int64_t im = (bc+ad) >> 16;
    return Q6_P_combine_RR(Q6_R_sat_P(im), Q6_R_sat_P(re));
    // return (re&0x0ffffffff) + (im << 32);
}
//(a+bi)*(c+di)=(ac+bd)+(bc-ad)i
inline cwfq16_t cwfq16_mul_conj(cwfq16_t v1, cwfq16_t v2)
{
    wfq16_t v1_re = cwfq16_re(v1);
    wfq16_t v1_im = cwfq16_im(v1);
    wfq16_t v2_re = cwfq16_re(v2);
    wfq16_t v2_im = cwfq16_im(v2);
    int64_t ac = Q6_P_mpy_RR(v1_re, v2_re);
    int64_t bd = Q6_P_mpy_RR(v1_im, v2_im);
    int64_t bc = Q6_P_mpy_RR(v1_im, v2_re);
    int64_t ad = Q6_P_mpy_RR(v1_re, v2_im);
    int64_t re = (ac+bd) >> 16;
    int64_t im = (bc-ad) >> 16;
    return Q6_P_combine_RR(Q6_R_sat_P(im), Q6_R_sat_P(re));
    // return (re&0x0ffffffff) + (im << 32);
}
inline cwfq8_t cwfq16_mul_conj2(cwfq16_t v1, cwfq16_t v2)
{
    wfq16_t v1_re = cwfq16_re(v1);
    wfq16_t v1_im = cwfq16_im(v1);
    wfq16_t v2_re = cwfq16_re(v2);
    wfq16_t v2_im = cwfq16_im(v2);
    int64_t ac = Q6_P_mpy_RR(v1_re, v2_re);
    int64_t bd = Q6_P_mpy_RR(v1_im, v2_im);
    int64_t bc = Q6_P_mpy_RR(v1_im, v2_re);
    int64_t ad = Q6_P_mpy_RR(v1_re, v2_im);
    int64_t re = (ac+bd) >> (16+8);
    int64_t im = (bc-ad) >> (16+8);
    return Q6_P_combine_RR(Q6_R_sat_P(im), Q6_R_sat_P(re));
    // return (re&0x0ffffffff) + (im << 32);
}
//(a+bi)*s=(a*s)+(b*s)i
inline cwfq16_t cwfq16_mul_scale(cwfq16_t v1, wfq16_t v2)
{
    wfq16_t v1_re = cwfq16_re(v1);
    wfq16_t v1_im = cwfq16_im(v1);
    int64_t as = Q6_P_mpy_RR(v1_re, v2);
    int64_t bs = Q6_P_mpy_RR(v1_im, v2);
    int64_t re = as >> 16;
    int64_t im = bs >> 16;
    return Q6_P_combine_RR(Q6_R_sat_P(im), Q6_R_sat_P(re));
}
//(a+bi)/(c+di)=(ac+bd)/(cc+dd)+(bc-ad)/(cc+dd)i
inline cwfq16_t cwfq16_div(cwfq16_t v1, cwfq16_t v2)
{
    wfq16_t v1_re = cwfq16_re(v1);
    wfq16_t v1_im = cwfq16_im(v1);
    wfq16_t v2_re = cwfq16_re(v2);
    wfq16_t v2_im = cwfq16_im(v2);

    int64_t ac = Q6_P_mpy_RR(v1_re, v2_re);
    int64_t bd = Q6_P_mpy_RR(v1_im, v2_im);
    int64_t bc = Q6_P_mpy_RR(v1_im, v2_re);
    int64_t ad = Q6_P_mpy_RR(v1_re, v2_im);
    int64_t cc = Q6_P_mpy_RR(v2_re, v2_re);
    int64_t dd = Q6_P_mpy_RR(v2_im, v2_im);
    int64_t tmp = (cc + dd) >> 16;

    int64_t re = (ac+bd)/tmp;
    int64_t im = (bc-ad)/tmp;
    return Q6_P_combine_RR(im, re);
    // return (re&0x0ffffffff) + (im << 32);
}

// N is log2(np)
template<int N>
inline cwfq16_t chfq_to_cwfq16(chfq_t v)
{
    int32_t re = Q6_R_sxth_R(v) << (16-N);
    int32_t im = Q6_R_asrh_R(v) << (16-N);
    return Q6_P_combine_RR(im, re);
}

inline cwfq16_t chfq8_to_cwfq16(chfq8_t v)
{
    wfq16_t re = Q6_R_sxth_R(v) << 8;
    wfq16_t im = Q6_R_asrh_R(v) << 8;
    return Q6_P_combine_RR(im, re);
}
inline chfq8_t cwfq16_to_chfq8(cwfq16_t v)
{
    int32_t re = cwfq16_re(v) >> 8;
    int32_t im = cwfq16_im(v) >> 8;
    return Q6_R_combine_RlRl(im, re);
}

inline cwfq16_t chfq9_to_cwfq16(chfq8_t v)
{
    wfq16_t re = Q6_R_sxth_R(v) << 7;
    wfq16_t im = Q6_R_asrh_R(v) << 7;
    return Q6_P_combine_RR(im, re);
}

inline cwfq16_t cwfq8_to_cwfq16(cwfq8_t v)
{
    int32_t re = cwfq8_re(v) << 8;
    int32_t im = cwfq8_im(v) << 8;
    return Q6_R_combine_RlRl(im, re);
}

//(a+bi)*(c+di)=(ac+bd)+(bc-ad)i
inline chfq8_t chfq8_mul_conj(chfq8_t v1, chfq8_t v2)
{
    hfq8_t v1_re = chfq8_re(v1);
    hfq8_t v1_im = chfq8_im(v1);
    hfq8_t v2_re = chfq8_re(v2);
    hfq8_t v2_im = chfq8_im(v2);
    int32_t ac = Q6_R_mpy_RlRl(v1_re, v2_re);
    int32_t bd = Q6_R_mpy_RlRl(v1_im, v2_im);
    int32_t bc = Q6_R_mpy_RlRl(v1_im, v2_re);
    int32_t ad = Q6_R_mpy_RlRl(v1_re, v2_im);
    int32_t re = (ac+bd) >> 8;
    int32_t im = (bc-ad) >> 8;
    return Q6_R_combine_RlRl(Q6_R_sath_R(im), Q6_R_sath_R(re));
    // return (re&0x0ffffffff) + (im << 32);
}
//(a+bi)*(c+di)=(ac+bd)+(bc-ad)i
inline cwfq8_t cwfq8_mul_conj(cwfq8_t v1, cwfq8_t v2)
{
    wfq8_t v1_re = cwfq8_re(v1);
    wfq8_t v1_im = cwfq8_im(v1);
    wfq8_t v2_re = cwfq8_re(v2);
    wfq8_t v2_im = cwfq8_im(v2);
    int64_t ac = Q6_P_mpy_RR(v1_re, v2_re);
    int64_t bd = Q6_P_mpy_RR(v1_im, v2_im);
    int64_t bc = Q6_P_mpy_RR(v1_im, v2_re);
    int64_t ad = Q6_P_mpy_RR(v1_re, v2_im);
    int64_t re = (ac+bd) >> 8;
    int64_t im = (bc-ad) >> 8;
    return Q6_P_combine_RR(Q6_R_sat_P(im), Q6_R_sat_P(re));
    // return (re&0x0ffffffff) + (im << 32);
}

typedef uint8_t ubfq7_t;

typedef int64_t cwfq_t;

#endif // QFE_H
